#include "CarControl.h"
#include "Delay.h"
#include "gec_init.h"
#include "gec_gpio.h"
#include "LED.h"


void SearchingPathInit() {
	//有障碍低电平，无障碍高电平
  Gec_GPIO_Init(GPIOG, GPIO_Pin_15, GPIO_MODE_IN_PULLUP);//HW DCMI_RESET
	Gec_GPIO_Init(GPIOC, GPIO_Pin_6, GPIO_MODE_IN_PULLUP);//HW DCMI_D0
	
	Gec_GPIO_Init(GPIOC, GPIO_Pin_7, GPIO_MODE_IN_PULLUP);//HW DCMI_D1
	Gec_GPIO_Init(GPIOC, GPIO_Pin_8, GPIO_MODE_IN_PULLUP);//HW DCMI_D2
}
int getHWLeft() {
  return Gec_GPIO_Get_Value(GPIOG, GPIO_Pin_15);
}

int getHWRight() {
  return Gec_GPIO_Get_Value(GPIOC, GPIO_Pin_6);
}


int getHWLeftDown() {
  return Gec_GPIO_Get_Value(GPIOC, GPIO_Pin_7);
}

int getHWRightDown() {
  return Gec_GPIO_Get_Value(GPIOC, GPIO_Pin_8);
}

void startSearchingPath() {
  int sl = getHWLeftDown();//循迹左
  int sr = getHWRightDown();//循迹右
  if(sl == 0 && sr == 0 ) { //左右探头都不在黑线中，直走
    carGoForward();
  } else if(sl == 1 && sr == 1) { //左右探头都在黑线中，停止
    carStop();
  } else if(sl == 1 && sr == 0) { //左探头在黑线中，右探头没有在黑线中
    carTurnLeft();
  } else if(sl == 0 && sr == 1) { //右探头在黑线中，左探头没有在黑线中
    carTurnRight();
  }
}

void startAvoidance() {
  int bl = getHWLeft();//避障左
  int br = getHWRight();//避障右
  //左侧有障碍，右侧无障碍
  if(bl==0 && br==1) {
    carTurnRight();
    Delay_ms(1500);
    carGoForward();
		
		
  }
  // 右侧有障碍，左侧无障碍
  if(br==0 && bl==1) {
    carTurnLeft();
    Delay_ms(200);
    carGoForward();
		
  }
  //正前方有障碍
  if(bl==0 && br==0) {
    carGoBack();
    Delay_ms(1500);
    carTurnLeft();
    Delay_ms(1500);
    carGoForward();
    //后退，掉头
  }
	
	
  carGoForward();

}

